
//      ******************************************************************
//      *                                                                *
//      *                Header file for MotionControl.c                 *
//      *                                                                *
//      *               Copyright (c) S. Reifel & Co, 2014               *
//      *                                                                *
//      ******************************************************************


// MIT License
// 
// Copyright (c) 2014 Stanley Reifel & Co.
// 
// Permission is hereby granted, free of charge, to any person obtaining a copy
// of this software and associated documentation files (the "Software"), to deal
// in the Software without restriction, including without limitation the rights
// to use, copy, modify, merge, publish, distribute, sublicense, and/or sell
// copies of the Software, and to permit persons to whom the Software is furnished
// to do so, subject to the following conditions:
// 
// The above copyright notice and this permission notice shall be included in all
// copies or substantial portions of the Software.
// 
// THE SOFTWARE IS PROVIDED "AS IS", WITHOUT WARRANTY OF ANY KIND, EXPRESS OR
// IMPLIED, INCLUDING BUT NOT LIMITED TO THE WARRANTIES OF MERCHANTABILITY,
// FITNESS FOR A PARTICULAR PURPOSE AND NONINFRINGEMENT. IN NO EVENT SHALL THE
// AUTHORS OR COPYRIGHT HOLDERS BE LIABLE FOR ANY CLAIM, DAMAGES OR OTHER
// LIABILITY, WHETHER IN AN ACTION OF CONTRACT, TORT OR OTHERWISE, ARISING FROM,
// OUT OF OR IN CONNECTION WITH THE SOFTWARE OR THE USE OR OTHER DEALINGS IN THE
// SOFTWARE.


#ifndef SMC_H
#define SMC_H

#include <arduino.h>
#include <stdlib.h>
#include <DigitalOut.h>
#include "TB67S109A.h"
#include "log.h"
// connect the stepper object to the IO pins
//  Enter:  stepPinNumber = IO pin number for the Step
//          directionPinNumber = IO pin number for the direction bit
//          enablePinNumber = IO pin number for the enable bit (LOW is enabled)
//            set to 0 if enable is not supported
//
// void MotionControl::connectToPins(byte stepPinNumber, byte directionPinNumber)
// {
//   //
//   // remember the pin numbers
//   //
//   stepPin = stepPinNumber;
//   directionPin = directionPinNumber;
  
//   //
//   // configure the IO bits
//   //
//   pinMode(stepPin, OUTPUT);
//   digitalWrite(stepPin, LOW);

//   pinMode(directionPin, OUTPUT);
//   digitalWrite(directionPin, LOW);
// }
#include "TB67S109A.h"

class MotionControl
{
public:
    
    MotionControl(PinName step_pin,toshiba::TB67S109A& tb67s109);
    ~MotionControl()=default;
    void setStepsPerMillimeter(float motorStepPerMillimeter);

    float getCurrentPositionInMillimeters();
    void setCurrentPositionInMillimeters(float currentPositionInMillimeter);
    void setSpeedInMillimetersPerSecond(float speedInMillimetersPerSecond);
    void setAccelerationInMillimetersPerSecondPerSecond(float accelerationInMillimetersPerSecondPerSecond);
    //bool moveToHomeInMillimeters(long directionTowardHome, float speedInMillimetersPerSecond, long maxDistanceToMoveInMillimeters, int homeLimitSwitchPin);
    void moveRelativeInMillimeters(float distanceToMoveInMillimeters);
    void setupRelativeMoveInMillimeters(float distanceToMoveInMillimeters);
    void moveToPositionInMillimeters(float absolutePositionToMoveToInMillimeters);
    void setupMoveInMillimeters(float absolutePositionToMoveToInMillimeters);
    float getCurrentVelocityInMillimetersPerSecond();
    

    long getCurrentPositionInSteps();
    void setCurrentPositionInSteps(long currentPositionInSteps);
    void setSpeedInStepsPerSecond(float speedInStepsPerSecond);
    void setAccelerationInStepsPerSecondPerSecond(float accelerationInStepsPerSecondPerSecond);
    //bool moveToHomeInSteps(long directionTowardHome, float speedInStepsPerSecond, long maxDistanceToMoveInSteps, int homeSwitchPin);
    void moveRelativeInSteps(long distanceToMoveInSteps);
    void setupRelativeMoveInSteps(long distanceToMoveInSteps);
    void moveToPositionInSteps(long absolutePositionToMoveToInSteps);
    void setupMoveInSteps(long absolutePositionToMoveToInSteps);
    float getCurrentVelocityInStepsPerSecond();

    bool processMovementUpEdge();
    bool processMovementDownEdge();


    // void enableStepper(void);
    // void disableStepper(void);
    bool determine_last_step();
    void setupStop();
    bool motionComplete();
     
    bool up_edge()
    {
      // if(determine_last_step()==false){
      //   return true;
      // }
      return (_counter++%4)==0;
    }

    void reset()
    {
      _step = 0;
      _counter = 0;
    }

    void toggle()
    {
      if(++_counter%4==0){
           _step = !_step;
      }
    }
  private:
    //byte stepPin;
    //byte directionPin;
    //toshiba::TB67S109A::DriverID _id;
    DigitalOut _step;
    toshiba::TB67S109A& _tb67s109;
    volatile bool _last_step;
    volatile bool _execute_rising_edge;
    volatile float desiredSpeed_InStepsPerSecond;
    volatile float acceleration_InStepsPerSecondPerSecond;

    volatile float stepsPerMillimeter;
    //volatile float stepsPerRevolution;

    volatile bool startNewMove;

    volatile float desiredStepPeriod_InUS;
    volatile long decelerationDistance_InSteps;

    volatile int direction_Scaler;

    volatile float ramp_InitialStepPeriod_InUS;
    volatile float ramp_NextStepPeriod_InUS;
    volatile unsigned long ramp_LastStepTime_InUS;

    volatile float acceleration_InStepsPerUSPerUS;
    volatile float currentStepPeriod_InUS;

    volatile long targetPosition_InSteps;
    volatile long currentPosition_InSteps;


    volatile unsigned long currentTime_InUS;
    volatile unsigned long periodSinceLastStep_InUS;
    volatile long distanceToTarget_InSteps;

    volatile unsigned char _counter;

    volatile uint8_t _my_counter;
};


// typedef struct
// {
//   union{
//     struct{
//         tb67s109a_t drv1;
//         tb67s109a_t drv2;
//         tb67s109a_t drv3;
//     };
//     uint8_t data[3];
//     tb67s109a_t drvs[3];
//   };
// }tb67s109a_set_t;
class SMCClock
{
public:
  SMCClock()=default;
  ~SMCClock()=default;
  static void generate()
  {
    
      cli();//stop interrupts
      TCCR1A = 0;// set entire TCCR1A register to 0
      TCCR1B = 0;// same for TCCR1B
      TCNT1  = 0;//initialize counter value to 0
      OCR1A = 255;//TOP = (16*10^6) / (1*1024) - 1 (must be <65536)
      // turn on CTC mode
      TCCR1B |= (1 << WGM12);
      // Set CS11 bits for 8 prescaler
      TCCR1B |= (1 << CS11);// | (1 << CS10);
      // enable timer compare interrupt
      //TIMSK1 |=  _BV(OCIE1A) | _BV(OCIE1B);


      TCCR2A = 0;// set entire TCCR1A register to 0
      TCCR2B = 0;// same for TCCR1B
      TCNT2  = 0;//initialize counter value to 0
      OCR2A = 255;// = (16*10^6) / (1*1024) - 1 (must be <65536)

      // turn on CTC mode
      TCCR2A |= (1 << WGM21);

      // Set CS21 bits for 8 prescaler
      TCCR2B |= (1 << CS21);// | (1 << CS10);
      // enable timer compare interrupt
      //TIMSK2 |= _BV(OCIE2A) ;

      sei();//allow interrupts
  }

  static void set_driver1_clock_enabled(bool enable)
  {
    enable ? TIMSK1 |= _BV(OCIE1A):TIMSK1 &= ~(_BV(OCIE1A));
  }
  static void set_driver2_clock_enabled(bool enable)
  {
    enable ? TIMSK1 |= _BV(OCIE1B):TIMSK1 &= ~(_BV(OCIE1B));
  }
  static void set_driver3_clock_enabled(bool enable)
  {
    enable ? TIMSK2 |= _BV(OCIE2A):TIMSK2 &= ~(_BV(OCIE2A));
  }
  static void set_driver4_clock_enabled(bool enable)
  {
    enable ? TIMSK2 |= _BV(OCIE2B):TIMSK2 &= ~(_BV(OCIE2B));
  }
private:

};

typedef struct
{
 union{
    struct{
        toshiba::tb67s109a_t device1;
        toshiba::tb67s109a_t device2;
        toshiba::tb67s109a_t device3;
    };
    uint8_t data[3];
    toshiba::tb67s109a_t devices[3];
  };
}tb67s109a_electrical_control_t;

class SMC
{
public:
  SMC(PinName pin_m1,PinName pin_m2,PinName pin_m3,ti::SN74AHC595& hc595):  
  _hc595(hc595),
  _tb67s109{{_tb67s109a_electrical_control.device1,hc595,&SMCClock::set_driver1_clock_enabled},
  {_tb67s109a_electrical_control.device2,hc595,&SMCClock::set_driver2_clock_enabled},
  {_tb67s109a_electrical_control.device3,hc595,&SMCClock::set_driver3_clock_enabled}},
  _mc{{pin_m1,_tb67s109[0]},{pin_m2,_tb67s109[1]},{pin_m3,_tb67s109[2]}}
  {
    
  }
  ~SMC()=default;

  void begin()
  {
    //memset(_tb67s109a_set.data,0,sizeof(_tb67s109a_set.data));
    _hc595.begin(_tb67s109a_electrical_control.data);
   
  }
  tb67s109a_electrical_control_t& tb67s109a_electrical_control()
  {
    return _tb67s109a_electrical_control;
  }
  // toshiba::TB67S109A& electronic_device1()
  // {
  //   return _tb67s109_m1;
  // }
  // toshiba::TB67S109A& electronic_device2()
  // {
  //   return _tb67s109_m2;
  // }
  // toshiba::TB67S109A& electronic_device3()
  // {
  //   return _tb67s109_m3;
  // }
    toshiba::TB67S109A& electronic_device(uint8_t id)
  {
    return _tb67s109[id];
  }
  void update_electrical_devices()
  {
    _hc595.updateRegisters(3);
  }
  void test_595()
  {
    for(;;){
      _tb67s109a_electrical_control.data[0] = 0xFF;
      _tb67s109a_electrical_control.data[1] = 0xFF;
      _tb67s109a_electrical_control.data[2] = 0xFF;
      _hc595.updateRegisters(3);

      delay(1000);
       _tb67s109a_electrical_control.data[0] = 0x00;
      _tb67s109a_electrical_control.data[1] = 0x00;
      _tb67s109a_electrical_control.data[2] = 0x00;
      _hc595.updateRegisters(3);

      delay(1000);
    }
  }
  // MotionControl& mc1()
  // {
  //   return _m1;
  // }
  // MotionControl& mc2()
  // {
  //   return _m2;
  // }
  // MotionControl& mc3()
  // {
  //   return _m3;
  // }
  MotionControl& mc(uint8_t id)
  {
    return _mc[id];
  }
private:
    ti::SN74AHC595& _hc595;
    tb67s109a_electrical_control_t _tb67s109a_electrical_control;
    // toshiba::TB67S109A _tb67s109_m1;
    // toshiba::TB67S109A _tb67s109_m2;
    // toshiba::TB67S109A _tb67s109_m3;
    toshiba::TB67S109A _tb67s109[3];
    // MotionControl _m1;
    // MotionControl _m2;
    // MotionControl _m3;
     MotionControl _mc[3];
};
// ------------------------------------ End ---------------------------------
#endif

